#include "PointCloud.h"
#include "Util.h"

using namespace cv;

static UINT nump = 0;

#define USE_DUPLICATE 1
#define RAY_ANGLE_THRESHOLD 2.0

bool Compare3D::operator()(cv::Point3d a, cv::Point3d b) const
{
	if (a.x == b.x)
	{
		if (a.y == b.y)
		{
			if (a.z == b.z)
			{
				return false;
			}
			else return a.z < b.z;
		}
		else return a.y < b.y;
	}
	else return a.x < b.x;
}

void CPointCloud::MergeKeys(SPoint& dst, SPoint& src)
{
	for (UINT i = 0; i < src.keys.size(); i++)
	{
		dst.keys.push_back(src.keys[i]);
	}
}

void CPointCloud::AddPoint(SPoint& point)
{
	if (USE_DUPLICATE)
	{
		//update color
		CCamera* pCam = point.keys[0].first;
		UINT KeyIndex = point.keys[0].second;
		point.color = pCam->GetColorOfKeyPoint(KeyIndex);

		point.pTrack->MarkPoint(_points.size());
		_points.push_back(point);
	}
	else
	{
		PointCloudData::iterator it = m_PointCloud.find(point.point);
		if (it != m_PointCloud.end())
		{
			///assuming there won't be any clashes
			SPoint& CurrPoint = it->second;
			MergeKeys(CurrPoint, point);
		}
		else
		{
			//new point
			m_PointCloud.insert(PointCloudEntryPair(point.point, point));
		}
	}
	
}

void CPointCloud::CreatePoints(const CImageMatch* pMatch, const CamerasVector& cams)
{
	CCamera& CamA = *(cams[0]);
	CCamera& CamB = *(cams[1]);
	for (UINT i = 0; i < pMatch->GetNumMatches(); i++)
	{
		SPoint newPoint;

		KeyPointMatch KeyPoints = pMatch->GetKeyPoints(i);
		KeyPointIndexMatch KeyPointsIndex = pMatch->GetKeyPointsIndex(i);
		newPoint.point = m_Trinangulator.Triangulate(KeyPoints.first, KeyPoints.second, CamA, CamB);

		//this keypoint correspond to a track get it from the image
		//todo find a better way to store this so it should be clearer how to retrieve and use this
		newPoint.pTrack = CamA.GetTrackFromKeyPointIndex(KeyPointsIndex.first);
		AddKeys(newPoint, tKeyData(cams[0], KeyPointsIndex.first));
		AddKeys(newPoint, tKeyData(cams[1], KeyPointsIndex.second));
		AddPoint(newPoint);
	}
}

UINT CPointCloud::GetViewMask(UINT NumCameras, char* vmask, double* projections, Point3d* points, int NumProjections, int* remap) const
{
	UINT NumberActivePoints = 0;
	ZeroMemory(vmask, Size() * NumCameras * sizeof(char));
	ZeroMemory(projections, NumProjections * 2 * sizeof(double));

	
	if (USE_DUPLICATE)
	{
		UINT ProjIndex = 0; //since the num of projections is different then numPoints* numcams we need to keep a different iterator for the projections

		for (int nPoint = 0; nPoint < Size(); nPoint++)
		{
			const SPoint& CurrPoint = _points[nPoint];
			const tKeyVector& CurrViews = CurrPoint.keys;

			if (!CurrViews.empty()) //check wheather we removed that point as an outlier
			{
				points[NumberActivePoints] = CurrPoint.point;
				remap[nPoint] = NumberActivePoints; //remapping index from point cloud to points array
				for (UINT i = 0; i < CurrViews.size(); i++)
				{
					const tKeyData& CurrKey = CurrViews[i];
					const CCamera* pCam = CurrKey.first;
					UINT CamIndex = pCam->GetAddedIndex();
					if (vmask[NumberActivePoints * NumCameras + CamIndex] == 0)
					{
						//only update if didn't already
						vmask[NumberActivePoints * NumCameras + CamIndex] = 1;

						Point2d projection = pCam->GetKeyPoint(CurrKey.second);

						projections[ProjIndex * 2 + 0] = projection.x;
						projections[ProjIndex * 2 + 1] = projection.y;
						ProjIndex++;
					}


				}

				//done preparing point increase point count
				NumberActivePoints++;
			}
			else
			{
				remap[nPoint] = -1;
			}
		}

		return NumberActivePoints;
	}
	else
	{
		//don't get here
		VISIO_ASSERT(0);
		return 0;
	}
}

int CPointCloud::Size() const
{
	if (USE_DUPLICATE)
	{
		return _points.size();
	}
	else
	{
		VISIO_ASSERT(0)
		return m_PointCloud.size();
	}
}

void CPointCloud::UpdatePoints( double* points, UINT NumPoints )
{
	for (UINT i = 0; i < NumPoints; i++)
	{
		Point3d p = Point3d(points[3*i], points[3*i + 1], points[3*i + 2]);
		_points[i].point = p;
	}
}

void CPointCloud::UpdatePoints( Point3d* points, UINT NumPoints, int* remap )
{
	for (UINT i = 0; i < NumPoints; i++)
	{
		int RemappedIndex = remap[i];
		if (RemappedIndex != -1)
		{
			Point3d p = points[RemappedIndex];
			_points[i].point = p;
		}
	}
}

bool CPointCloud::empty() const
{
	return _points.empty();
}

cv::Point3d CPointCloud::Get3DPoint( UINT index ) const
{
	VISIO_ASSERT(!_points[index].keys.empty())
	return _points[index].point;
}

void CPointCloud::AddKeyData(UINT index, CCamera* pCam, UINT keyPointIndex)
{
	AddKeys(_points[index], tKeyData(pCam, keyPointIndex), index);
}

void CPointCloud::AddPoints(CamerasVector& cams, CImageDataBase& ImageDataBase)
{
	
	ImageDataPtrVector ImPtrVec;

	for (UINT i = 0; i < cams.size(); i++)
	{
		// make a vector of the image data for GetTracksShared() call
		ImPtrVec.push_back(cams[i]->GetImageData());
	}

	SharedViewVector SharedViews = ImageDataBase.GetTracksShared(ImPtrVec);

	for (UINT i = 0; i < SharedViews.size(); i++)
	{
		ViewVector CurrView = SharedViews[i].views;
		int NumPoints = (int)CurrView.size();

		if (NumPoints >= MIN_VIEWS_FOR_TRIANGULATION)
		{

			// now prepare all data for triangulation

			//todo: remove the dynamic memory allocation here
			Point2d* projs = new Point2d[CurrView.size()];
			CCamera** ppCams = new CCamera*[cams.size()];

			for (UINT j = 0; j < CurrView.size(); j++)
			{
				CCamera* pCam = cams[CurrView[j].first];
				ppCams[j] = pCam;
				projs[j] = pCam->GetKeyPoint(CurrView[j].second);
			}

			if (IsWellConditioned(ppCams, projs, CurrView.size()))
			{
				double error = 0;

				Point3d point = m_Trinangulator.TrinagulateN(projs, ppCams, NumPoints, error);

				if (error < MAX_REPROJECTION_ERROR)

				{
					SPoint newPoint;

					newPoint.point = point;
					newPoint.pTrack = SharedViews[i].pTrack;
					for (UINT j = 0; j < CurrView.size(); j++)
					{
						AddKeys(newPoint, tKeyData(ppCams[j], CurrView[j].second));
					}

					AddPoint(newPoint);
				}
			}
			

			delete[] ppCams;
			delete[] projs;
		}
	}
}

void CPointCloud::AddKeys(SPoint& point, tKeyData KeyData, UINT PointIndex)
{
	KeyData.first->MarkPoint(KeyData.second, PointIndex); //tell the keypoint which index is her parent 3d point
	point.keys.push_back(KeyData);
}

void CPointCloud::AddKeys(SPoint& point, tKeyData KeyData)
{
	AddKeys(point, KeyData, _points.size());
}

bool CPointCloud::IsWellConditioned(CCamera** ppCams, Point2d* projs, UINT NumPoints)
{
	for (UINT i = 0; i < NumPoints; i++)
	{
		for (UINT j = i+1; j < NumPoints; j++)
		{
			Point2d p = projs[i];
			Point2d q = projs[j];
			
			CCamera* camA = ppCams[i];
			CCamera* camB = ppCams[j];

			double angle = ComputeRayAngle(p, q, camA, camB);

			if (ToDegree(angle) >= RAY_ANGLE_THRESHOLD)
			{
				return true;
			}
		}
	}

	return false;

}

double CPointCloud::ComputeRayAngle(Point2d p, Point2d q, CCamera* CamA, CCamera* CamB)
{

	// we want to compute the angle between the rays which are (X_p - c_A) and (X_q - c_B)
	// we can calculate it from the formula R(X_p - c_A) = p => X_p - c_A = Rt*p
	SCameraParams CamAParams = CamA->GetParameters();
	SCameraParams CamBParams = CamB->GetParameters();

	Mat p3D = INIT_MAT(3, 1, p.x , p.y, 1);
	Mat q3D = INIT_MAT(3, 1, q.x , q.y, 1);

	Mat p3DNormalized = CamAParams.K.inv() * p3D;
	Mat q3DNormalized = CamBParams.K.inv() * q3D;

	Mat p2DNormalized =  Utils::Project3D(p3DNormalized);
	Mat q2DNormalized =  Utils::Project3D(q3DNormalized);

	double p2D[] = {0, 0, -1.0};
	double q2D[] = {0, 0, -1.0};

	memcpy(p2D, GetMatrixDataAsDouble(p2DNormalized), sizeof(double) * 2);
	memcpy(q2D, GetMatrixDataAsDouble(q2DNormalized), sizeof(double) * 2);

	Mat pV = INIT_MAT_ARRAY(3, 1, p2D);
	Mat qV = INIT_MAT_ARRAY(3, 1, q2D);

	//this will give us (X-c)
	Mat Rt_pV = CamAParams.R.t() * pV;
	Mat Rt_qV = CamBParams.R.t() * qV;

	double dot = Utils::Dot(Rt_pV.t(), Rt_qV);
	double mag = norm(Rt_pV) * norm(Rt_qV);

	double angle = acos(Clamp(dot / mag, -1.0 + 1.0e-8, 1.0 - 1.0e-8));

	return angle;
}

int CPointCloud::GetNumberOfProjections() const
{
	int NumProj = 0;
	for (int i = 0; i < Size(); i++)
	{
		NumProj += _points[i].keys.size();
	}

	return NumProj;
}

void CPointCloud::SortKeyData(tKeyVector& keys)
{
	for (UINT i = 0; i < keys.size(); i++)
	{
		for (UINT j = keys.size() - 1; j > i; j--)
		{
			if (keys[j-1].first->GetAddedIndex() > keys[j].first->GetAddedIndex())
			{
				std::swap(keys[j-1], keys[j]);	
			}
		}
	}
}

void CPointCloud::CheckViews()
{
	//were gonna make sure here that all the views in point cloud are in order. meaning the views of a certain points their cam indexes are in increasing order
	for (int i = 0; i < _points.size(); i++)
	{
		SortKeyData(_points[i].keys);
	}
}

void CPointCloud::VerifyViewsWithBundler()
{
	std::vector<std::vector<std::pair<UINT, UINT>>> ViewsVec; 
	int NumViews = 0;
	FILE* f = fopen("ptviews.out", "r");
	fscanf(f, "%d", &NumViews);
	for (int i = 0; i < NumViews; i++)
	{
		std::vector<std::pair<UINT, UINT>> Views;
		int NumEle = 0;
		fscanf(f, "%d", &NumEle);
		for (int j = 0; j < NumEle; j++)
		{
			int CamIndex, KeyPointIndex;
			fscanf(f, "%d %d", &CamIndex, &KeyPointIndex);
			Views.push_back(std::pair<UINT, UINT>(CamIndex, KeyPointIndex));
		}
		ViewsVec.push_back(Views);
	}

	for (UINT i = 0; i < ViewsVec.size(); i++)
	{
		bool Found = false;
		for (UINT k = 0; k < _points.size(); k++)
		{
			if (ViewsVec[i].size() == _points[k].keys.size())
			{
				for (UINT j = 0; j < ViewsVec[i].size(); j++)
				{
					if (ViewsVec[i][j].first == _points[k].keys[j].first->GetAddedIndex() && ViewsVec[i][j].second == _points[k].keys[j].second)
					{
						Found = true;
						continue;
					}
					else
					{
						Found = false;
						break;
					}
				}
			}
			if (Found)
			{
				break;
			}
		}

		VISIO_ASSERT(Found);
		
	}
}

void CPointCloud::RemoveOutliers( OutliersSet outliers )
{
	for (OutliersSet::iterator it = outliers.begin(); it != outliers.end(); it++) 
	{
		int idx = *it;
		SPoint& point = _points[idx];
		point.pTrack->RemoveMark();
		for (UINT i = 0; i < point.keys.size(); i++)
		{
			CCamera* pCam = point.keys[i].first;
			pCam->RemoveMark(point.keys[i].second);
		}

		point.keys.clear();
	}
}

CPointCloudMeshInstance* CPointCloud::CreateMeshInstance()
{
	UINT NumPoints = Size();
	double* points = new double[NumPoints*3];
	float* colors = new float[NumPoints * 3];
	for (UINT i = 0; i < NumPoints; i++)
	{
		SPoint CurrPoint = _points[i];
		points[3*i] = CurrPoint.point.x;
		points[3*i + 1] = CurrPoint.point.y;
		points[3*i + 2] = CurrPoint.point.z;

		colors[3 * i] = CurrPoint.color.r;
		colors[3 * i + 1] = CurrPoint.color.g;
		colors[3 * i + 2] = CurrPoint.color.b;
		
	}

	CPointCloudMesh* pMesh = new CPointCloudMesh(points, colors, NumPoints);
	return pMesh->CreateInstance();
}

void CPointCloud::PrintToFile(FILE* pFile) const
{
	//format per point:
	//3d coords
	//color in rgb8
	//number of views currently not used
	//image index keypoint index keypoint coords currently not used
	for (int i = 0; i < Size(); i++)
	{
		SPoint point = _points[i];
		KeyColor col = point.color;
		fprintf(pFile,  "%.15e %.15e %.15e\n", point.point.x, point.point.y, point.point.z);
		
		fprintf(pFile, "%d %d %d\n", FLOAT_TO_UNORM(col.r), FLOAT_TO_UNORM(col.g), FLOAT_TO_UNORM(col.b));
		fprintf(pFile, "%d\n", 0);
	}
}